#include "Aircraft.h"

Aircraft::Aircraft()
{
    for (int i=0;i<8;i++){_in[i]=1500;}
    for (int i=0;i<4;i++){_out[i]=1500;}
}

void Aircraft::init(RC1 *sysrc, AP_PeriodicProcess *pdpr, AP_IMU_INS &imu, AP_DCM &dcm, MyGPS &gps)
{
	rc = sysrc;
	
	updateCalibration();
	
	ap.init(this);
	sensors.init(pdpr,imu,dcm,gps);
	Serial.println("DONE with Aircraft init!");
	digitalWrite(35, LOW);
	digitalWrite(36, LOW);
	digitalWrite(37, HIGH);
}

void Aircraft::update()
{
	updateRCin();
    sensors.update();
	ap.run();
    //runOffInputs();
	updateRCout();
}

void Aircraft::updateRCin()
{
    if (rc->getState() != 1){return;}
	for(int i = 0; i < 8; i++)
	{_in[i] = rc->inputCh(i);}
}

void Aircraft::updateRCout()
{
    if (rc->getState() != 1){return;}
    for(int i = 0; i < 8; i++)
	{rc->outputCh(i, _out[i]);}
}

int Aircraft::RCin(int ch){return _in[ch-1];}
void Aircraft::RCout(int ch, int val){_out[ch-1]=val;}
int Aircraft::getRCout(int ch){return _out[ch-1];}
void Aircraft::runOffInputs()
{
    for (int i=0;i<4;i++){_out[i]=_in[i];}
	/*
    Serial.print("XCH: ");
    for(int i = 0; i < 8; i++)
    {
        Serial.print((uint16_t)_in[i]);
        Serial.print(", ");
    }
    Serial.println();*/
}

void Aircraft::updateCalibration()
{
	SservoAllCalib svoCalib = EEPROMmanager::readServoCalibrations();
	servoMax[0] = svoCalib.ail.max;
	servoMin[0] = svoCalib.ail.min;
	servoInv[0] = svoCalib.ail.inverted;
	servoMax[1] = svoCalib.elev.max;
	servoMin[1] = svoCalib.elev.min;
	servoInv[1] = svoCalib.elev.inverted;
	servoMax[2] = svoCalib.thr.max;
	servoMin[2] = svoCalib.thr.min;
	servoInv[2] = svoCalib.thr.inverted;
	servoMax[3] = svoCalib.rud.max;
	servoMin[3] = svoCalib.rud.min;
	servoInv[3] = svoCalib.rud.inverted;
}

void Aircraft::servoTest()
{
	svoRoll(100);
	updateRCout();
	delay(1000);
	svoRoll(-100);
	updateRCout();
	delay(1000);
	svoRoll(0);
	svoPitch(100);
	updateRCout();
	delay(1000);
	svoPitch(-100);
	updateRCout();
	delay(1000);
	svoPitch(0);
	svoRudder(100);
	updateRCout();
	delay(1000);
	svoRudder(-100);
	updateRCout();
	delay(1000);
	svoRudder(0);
	updateRCout();
}

void Aircraft::mapServo(int num, int defl)
{
	if (defl<-100){defl=-100;}
	else if (defl>100){defl=100;}
	if (servoInv[0])
	{_out[num] = map(defl, -100, 100, servoMin[num], servoMax[num]);}
	else
	{_out[num] = map(defl, 100, -100, servoMin[num], servoMax[num]);}
}

void Aircraft::svoRoll(int defl){mapServo(0,defl);}
void Aircraft::svoPitch(int defl){mapServo(1,defl);}
void Aircraft::throttleCmd(int power)
{
	if (power<0){power=0;}
	else if (power>100){power=100;}
	if (servoInv[2])
	{_out[2] = map(power, 0, 100, servoMin[2], servoMax[2]);}
	else
	{_out[2] = map(power, 100, 0, servoMin[2], servoMax[2]);}
}
void Aircraft::svoRudder(int defl){mapServo(3,defl);}
void Aircraft::svoInvert(int ch){servoInv[ch-1]^=1;saveSvoPresets();}
void Aircraft::svoInvert(int ch,bool to){servoInv[ch-1]=to;saveSvoPresets();}
bool Aircraft::svoGetInvert(int ch){return servoInv[ch-1];}
int Aircraft::getSvoMin(int ch){return servoMin[ch-1];}
int Aircraft::getSvoMax(int ch){return servoMax[ch-1];}
void Aircraft::setSvoMax(int ch, int to)
{
	ch--;
	if (servoMin[ch]>to){servoMin[ch]=to;}
	servoMax[ch]=to;
	saveSvoPresets();
}
void Aircraft::setSvoMin(int ch, int to)
{
	ch--;
	if (servoMin[ch]>to){servoMax[ch]=to;}
	servoMin[ch]=to;
	saveSvoPresets();
}
void Aircraft::setSvoRange(int ch, int min, int max)
{
	ch--;
	if (max<min){max^=min;min^=max;max^=min;}
	servoMin[ch]=min;
	servoMax[ch]=max;
	saveSvoPresets();
}

void Aircraft::saveSvoPresets()
{
	SservoAllCalib calib;
	calib.ail.min = servoMin[0];
	calib.ail.max = servoMax[0];
	calib.ail.inverted = servoInv[0];
	calib.elev.min = servoMin[1];
	calib.elev.max = servoMax[1];
	calib.elev.inverted = servoInv[1];
	calib.thr.min = servoMin[2];
	calib.thr.max = servoMax[2];
	calib.thr.inverted = servoInv[2];
	calib.rud.min = servoMin[3];
	calib.rud.max = servoMax[3];
	calib.rud.inverted = servoInv[3];
	EEPROMmanager::writeServoCalibrations(calib);
}
